#include "MBOctreeNode.h"
#include "MBTriangle.h"

// Fast triangle-box intersection test algorithms by Tomas Moller
#include "Tribox3.h"

#include <iostream>
#include <math.h>

using std::cout;
using std::endl;
using std::max;
using std::min;
using std::pair;
using std::set;
using std::vector;

const MBuint MAX_NUMBER_OF_CHILDREN = 8u;

#ifdef X
	#undef X
#endif
#ifdef Y
	#undef Y
#endif
#ifdef Z
	#undef Z
#endif

const MBuint X = 0u;
const MBuint Y = 1u;
const MBuint Z = 2u;

/**
  *	Constructs a new MBOctreeNode with the given parameters
  *
  * @param min min-point of this node
  * @param max max-point of this node
  * @param level level number of this node
  * @param maxLevels maximum number of levels in the Octree
  */
MBOctreeNode::MBOctreeNode( const MBVec3 &min, const MBVec3 &max, const MBuint &level, const MBuint &maxLevels )
{
	mMin = min;
	mMax = max;
	mSideLengths = max - min;
	mMidPoint = (max + min) * 0.5f;
	mLevel = level;
	mMaxLevels = maxLevels;

	mChildren = new MBOctreeNode*[MAX_NUMBER_OF_CHILDREN];

	for (int i = 0; i < MAX_NUMBER_OF_CHILDREN; i++)
	{
		mChildren[i] = 0;
	}

	mTriangleIndices = 0;
}

/**
  *	Destroys this MBOctreeNode object and all of its children
  */
MBOctreeNode::~MBOctreeNode( void )
{
	for (int i = 0; i < 8; i++)
	{
		if (mChildren[i]) delete mChildren[i];
	}

	delete [] mChildren;

	if (mTriangleIndices)
	{
		delete mTriangleIndices;
	}
}

/**
  *	Returns the number of triangles inside this MBOctreeNode object
  *
  * @return number of triangles inside this MBOctreeNode object
  */
MBuint MBOctreeNode::GetNumberOfTriangles( void ) const
{
	MBuint result = 0;

	if (mTriangleIndices) result += mTriangleIndices->size();

	for (MBuint i = 0; i < MAX_NUMBER_OF_CHILDREN; i++)
	{
		if (mChildren[i]) result += mChildren[i]->GetNumberOfTriangles();
	}

	return result;
}

/**
  *	Adds one triangle to this MBOctreeNode
  *
  * @param triangleIndex index of the triangle that is added
  * @param trianglePoints const pointer to the array with the three triangle points
  */
void MBOctreeNode::AddTriangle( const MBuint &triangleIndex, const MBVec3 *trianglePoints )
{
	MBVec3 triangle_center( (trianglePoints[0] + trianglePoints[1] + trianglePoints[2]) / 3.0 );
	MBVec3 max_offset = mMax - triangle_center;
	MBVec3 min_offset = mMin - triangle_center;
	MBVec3 mid_point_offset = mMidPoint - triangle_center;

	MBVec3 triangle_max(
		max( max( trianglePoints[0][X], trianglePoints[1][X] ), trianglePoints[2][X] ),
		max( max( trianglePoints[0][Y], trianglePoints[1][Y] ), trianglePoints[2][Y] ),
		max( max( trianglePoints[0][Z], trianglePoints[1][Z] ), trianglePoints[2][Z] ) );

	MBVec3 triangle_min(
		min( min( trianglePoints[0][X], trianglePoints[1][X] ), trianglePoints[2][X] ),
		min( min( trianglePoints[0][Y], trianglePoints[1][Y] ), trianglePoints[2][Y] ),
		min( min( trianglePoints[0][Z], trianglePoints[1][Z] ), trianglePoints[2][Z] ) );

	MBVec3 halved_triangle_dims = (triangle_max - triangle_min) * 0.5;

	// Test if the bounding box of the triangle is completely outside the node
	if (max_offset[X] < -halved_triangle_dims[X] ||
		max_offset[Y] < -halved_triangle_dims[Y] ||
		max_offset[Z] < -halved_triangle_dims[Z] ||
		min_offset[X] > halved_triangle_dims[X] ||
		min_offset[Y] > halved_triangle_dims[Y] ||
		min_offset[Z] > halved_triangle_dims[Z])
	{
		// Triangle is completely outside
		return;
	}

	MBVec3 childSideLengths = mSideLengths * 0.5;

	if (mLevel == mMaxLevels)
	{
		MBVec3 box_h = (mMax - mMin) * 0.5;

		const double *tri_vertices[] = { &trianglePoints[0][X], &trianglePoints[1][X], &trianglePoints[2][X] };

		if (triBoxOverlap( &mMidPoint[X], &box_h[X], tri_vertices ))
		{
			if(!mTriangleIndices) mTriangleIndices = new vector<MBuint>();

			mTriangleIndices->push_back( triangleIndex );
		}

		return;

// 		float box_center[3] = {
// 			static_cast<float>( mMidPoint[X] ),
// 			static_cast<float>( mMidPoint[Y] ),
// 			static_cast<float>( mMidPoint[Z] ) };

		
// 		float box_half_size[3] = {
// 			static_cast<float>( box_h[X] ),
// 			static_cast<float>( box_h[Y] ),
// 			static_cast<float>( box_h[Z] ) };

// 		float tri_verts[3][3] = {
// 			{ static_cast<float>( trianglePoints[0][X] ), static_cast<float>( trianglePoints[0][Y] ), static_cast<float>( trianglePoints[0][Z] ) },
// 			{ static_cast<float>( trianglePoints[1][X] ), static_cast<float>( trianglePoints[1][Y] ), static_cast<float>( trianglePoints[1][Z] ) },
// 			{ static_cast<float>( trianglePoints[2][X] ), static_cast<float>( trianglePoints[2][Y] ), static_cast<float>( trianglePoints[2][Z] ) } };
	}

	// Test if the bounding box of the sphere is intersecting with the front, the back or both
	if (mid_point_offset[Z] < -halved_triangle_dims[Z])
	{
		// just intersecting the front

		// Test if the bounding box of the sphere is intersecting with the top, the bottom or both

		if (mid_point_offset[Y] < -halved_triangle_dims[Y])
		{
			// just intersecting the top

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with ftr and ftl

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

			} else
			{
				// intersection with ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );
			}
		} else if (mid_point_offset[Y] < halved_triangle_dims[Y])
		{
			// intersection with top and bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with ftr and fbr

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with ftr, ftl, fbr and fbl

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );

			} else
			{
				// intersection with ftl and fbl

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );
			}
	} else
		{
			// just intersecting the bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with fbr and fbl

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );
			} else
			{
				// intersection with fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );
			}
		}
	} else if (mid_point_offset[Z] < halved_triangle_dims[Z])
	{
		// intersection with front and back

		// Test if the bounding box of the sphere is intersecting with the top, the bottom or both

		if (mid_point_offset[Y] < -halved_triangle_dims[Y])
		{
			// just intersecting the top

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with ftr and btr

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with ftr, ftl, btr and btl

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

			} else
			{
				// intersection with ftl and btl

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );
			}
		} else if (mid_point_offset[Y] < halved_triangle_dims[Y])
		{
			// intersection with top and bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with ftr, fbr, btr and bbr

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with ALL nodes

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// ftr
				MBVec3 ftrMin = mMidPoint;
				MBVec3 ftrMax = mMax;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftrMin, ftrMax, MB_OCTREE_NODE_CHILD_ID_FTR );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

			} else
			{
				// intersection with ftl, fbl, btl and bbl

				// ftl
				MBVec3 ftlMin = MBVec3( mMidPoint[X] - childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				MBVec3 ftlMax = MBVec3( mMax[X] - childSideLengths[X], mMax[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, ftlMin, ftlMax, MB_OCTREE_NODE_CHILD_ID_FTL );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			}
		} else
		{
			// just intersecting the bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with fbr and bbr

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with fbr, fbl, bbr and bbl

				// fbr
				MBVec3 fbrMin = MBVec3( mMidPoint[X], mMidPoint[Y] - childSideLengths[Y], mMidPoint[Z] );
				MBVec3 fbrMax = MBVec3( mMax[X], mMax[Y] - childSideLengths[Y], mMax[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fbrMin, fbrMax, MB_OCTREE_NODE_CHILD_ID_FBR );

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );

			} else
			{
				// intersection with fbl and bbl

				// fbl
				MBVec3 fblMin = MBVec3( mMin[X], mMin[Y], mMin[Z] + childSideLengths[Z] );
				MBVec3 fblMax = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] + childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, fblMin, fblMax, MB_OCTREE_NODE_CHILD_ID_FBL );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			}
		}

	} else
	{
		// just intersecting the back

		// Test if the bounding box of the sphere is intersecting with the top, the bottom or both

		if (mid_point_offset[Y] < -halved_triangle_dims[Y])
		{
			// just intersecting the top

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with btr and btl

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

			} else
			{
				// intersection with btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );
			}
		} else if (mid_point_offset[Y] < halved_triangle_dims[Y])
		{
			// intersection with top and bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with btr and bbr

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with btr, btl, bbr and bbl

				// btr
				MBVec3 btrMin = MBVec3( mMidPoint[X], mMidPoint[Y], mMidPoint[Z] - childSideLengths[Z] );
				MBVec3 btrMax = MBVec3( mMax[X], mMax[Y], mMax[Z] - childSideLengths[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btrMin, btrMax, MB_OCTREE_NODE_CHILD_ID_BTR );

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			} else
			{
				// intersection with btl and bbl

				// btl
				MBVec3 btlMin = MBVec3( mMin[X], mMin[Y] + childSideLengths[Y], mMin[Z] );
				MBVec3 btlMax = MBVec3( mMidPoint[X], mMidPoint[Y] + childSideLengths[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, btlMin, btlMax, MB_OCTREE_NODE_CHILD_ID_BTL );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			}
		} else
		{
			// just intersecting the bottom

			// Test if the bounding box of the sphere is intersecting with the right, the left or both

			if (mid_point_offset[X] < -halved_triangle_dims[X])
			{
				// intersection with bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

			} else if (mid_point_offset[X] < halved_triangle_dims[X])
			{
				// intersection with bbr and bbl

				// bbr
				MBVec3 bbrMin = MBVec3( mMin[X] + childSideLengths[X], mMin[Y], mMin[Z] );
				MBVec3 bbrMax = MBVec3( mMidPoint[X] + childSideLengths[X], mMidPoint[Y], mMidPoint[Z] );
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bbrMin, bbrMax, MB_OCTREE_NODE_CHILD_ID_BBR );

				// bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			} else
			{
				// intersection with bbl
				MBVec3 bblMin = mMin;
				MBVec3 bblMax = mMidPoint;
				ManagePotentialTriangleIntersection( trianglePoints, triangleIndex, bblMin, bblMax, MB_OCTREE_NODE_CHILD_ID_BBL );
			}
		}
	}
}

/**
  *	Returns true if any triangle in this Octree node is intersecting with the given
  * sight frustum, false otherwise.
  *
  * @param sightFrustum sight frustum under consideration
  * @param triangleVertexIndices pointer to the three vertex positions of the triangle
  * @param triangles const pointer to the vector with all the pointers to the MBTriangle objects
  * @param vertexCoordinates const pointer to the vector with all the vertex positions, stored as pointers to double arrays
  * @return true if any triangle in the Octree is intersecting with the given sight frustum, false otherwise.
  */
bool MBOctreeNode::IsOtherTriangleIntersectingSightFrustum(
	const MBTriangleSightFrustum &sightFrustum,
	const MBuint *triangleVertexIndices,
	const vector<MBTriangle*> *triangles,
	const vector<double*> *vertexCoordinates )
	//set<MBuint> &alreadyCheckedTriangleIndices )
{
	bool intersection = false;

	// Test for intersection

	intersection = sightFrustum.IsIntersectingWithAxisAlignedBox( mMin, mMax );

	if (intersection)
	{
		// The own triangles
		if (mTriangleIndices)
		{
			vector<MBuint>::iterator it;
			const MBuint *t_v_i = 0;
			const vector<double*> &v = *vertexCoordinates;
			pair<set<MBuint>::iterator, bool> result;

			for (it = mTriangleIndices->begin(); it != mTriangleIndices->end(); it++)
			{
				t_v_i = (*triangles)[*it]->GetVertexIndices();

				if (t_v_i == triangleVertexIndices ||
					t_v_i[0] == triangleVertexIndices[0] ||
					t_v_i[1] == triangleVertexIndices[0] ||
					t_v_i[2] == triangleVertexIndices[0] ||
					t_v_i[0] == triangleVertexIndices[1] ||
					t_v_i[1] == triangleVertexIndices[1] ||
					t_v_i[2] == triangleVertexIndices[1] ||
					t_v_i[0] == triangleVertexIndices[2] ||
					t_v_i[1] == triangleVertexIndices[2] ||
					t_v_i[2] == triangleVertexIndices[2])
				{
					// This is the triangle itself or a neighbor
					continue;
				}

				const MBVec3 triangle_points[3] = {
					MBVec3( v[t_v_i[0]][X], v[t_v_i[0]][Y], v[t_v_i[0]][Z] ),
					MBVec3( v[t_v_i[1]][X], v[t_v_i[1]][Y], v[t_v_i[1]][Z] ),
					MBVec3( v[t_v_i[2]][X], v[t_v_i[2]][Y], v[t_v_i[2]][Z] ) };

				if (sightFrustum.IsPointInside( triangle_points[0] )) return true;
				if (sightFrustum.IsPointInside( triangle_points[1] )) return true;
				if (sightFrustum.IsPointInside( triangle_points[2] )) return true;

// 				if (alreadyCheckedTriangleIndices.find( *it ) != alreadyCheckedTriangleIndices.end())
// 				{
// 					// This triangle was already checked
// 					continue;
// 				}

				// OK, no point is inside, but it can still be the case, that the
				// triangle intersects with the frustum -> check every single triangle
				// of the frustum for intersection

				if (sightFrustum.IsIntersectingWithTriangle( triangle_points )) return true;
// 
// 				alreadyCheckedTriangleIndices.insert( *it );
			}
		}

		if (mLevel == mMaxLevels)
		{
			return false;
		}

		// The Triangles of the children
		for (MBuint i = 0; i < MAX_NUMBER_OF_CHILDREN; i++)
		{
			if (mChildren[i]) 
			{
				if (mChildren[i]->IsOtherTriangleIntersectingSightFrustum( sightFrustum, triangleVertexIndices, triangles, vertexCoordinates ))//, alreadyCheckedTriangleIndices ))
				{
					return true;
				}
			}
		}
	}

	return false;
}

/**
  *	Returns true if any triangle in this Octree node is intersecting with the given
  * sight triangle, false otherwise.
  *
  * @param sightTriangle sight triangle under consideration
  * @param edgeVertexIndices pointer to the two vertex positions of the edge
  * @param triangles const pointer to the vector with all the pointers to the MBTriangle objects
  * @param vertexCoordinates const pointer to the vector with all the vertex positions, stored as pointers to double arrays
  * @return true if any triangle in the Octree is intersecting with the given sight triangle, false otherwise.
  */
bool MBOctreeNode::IsOtherTriangleIntersectingSightTriangle(
	const MBTriangleSightTriangle &sightTriangle,
	const MBuint *edgeVertexIndices,
	const std::vector<MBTriangle*> *triangles,
	const std::vector<double*> *vertexCoordinates )
{
	bool intersection = false;

	// Test for intersection

	intersection = sightTriangle.IsIntersectingWithAxisAlignedBox( mMin, mMax );

	if (intersection)
	{
		// The own triangles
		if (mTriangleIndices)
		{
			vector<MBuint>::iterator it;
			const MBuint *t_v_i = 0;
			const vector<double*> &v = *vertexCoordinates;

			for (it = mTriangleIndices->begin(); it != mTriangleIndices->end(); it++)
			{
				t_v_i = (*triangles)[*it]->GetVertexIndices();

				if (t_v_i[0] == edgeVertexIndices[0] ||
					t_v_i[1] == edgeVertexIndices[0] ||
					t_v_i[2] == edgeVertexIndices[0] ||
					t_v_i[0] == edgeVertexIndices[1] ||
					t_v_i[1] == edgeVertexIndices[1] ||
					t_v_i[2] == edgeVertexIndices[1])
				{
					// This is the triangle itself or a neighbor
					continue;
				}
				const MBVec3 triangle_points[3] = {
					MBVec3( v[t_v_i[0]][X], v[t_v_i[0]][Y], v[t_v_i[0]][Z] ),
					MBVec3( v[t_v_i[1]][X], v[t_v_i[1]][Y], v[t_v_i[1]][Z] ),
					MBVec3( v[t_v_i[2]][X], v[t_v_i[2]][Y], v[t_v_i[2]][Z] ) };

				if (sightTriangle.IsIntersectingWithTriangle( triangle_points )) return true;
			}
		}

		if (mLevel == mMaxLevels)
		{
			return false;
		}

		// The Triangles of the children
		for (MBuint i = 0; i < MAX_NUMBER_OF_CHILDREN; i++)
		{
			if (mChildren[i]) 
			{
				if (mChildren[i]->IsOtherTriangleIntersectingSightTriangle( sightTriangle, edgeVertexIndices, triangles, vertexCoordinates ))
				{
					return true;
				}
			}
		}
	}

	return false;
}

/**
  *	Manages a potential triangle intersection, which means that it looks if the
  * given triangle is really intersecting the child node defined by cuboidMin
  * and cuboidMax. If it does so, it is added to to this child node.
  *
  * @param trianglePoints const pointer to the array with the three triangle points
  * @param triangleIndex triangle index
  * @param cuboidMin min-point of the child node that is intersecting with the bounding box of the triangle
  * @param cuboidMax max-point of the child node that is intersecting with the bounding box of the triangle
  * @param childID ID of the child node that is intersecting with the bounding box of the triangle
  */
void MBOctreeNode::ManagePotentialTriangleIntersection(
	const MBVec3 *trianglePoints,
	const MBuint &triangleIndex,
	const MBVec3 &cuboidMin,
	const MBVec3 &cuboidMax,
	const MBOctreeNodeChildID &childID )
{
	//MBVec3 box_h = (mMax - mMin) * 0.5;
	MBVec3 box_h = (cuboidMax - cuboidMin) * 0.5;
	const double *tri_vertices[] = { &trianglePoints[0][X], &trianglePoints[1][X], &trianglePoints[2][X] };

	MBVec3 mid_point = (cuboidMax + cuboidMin) * 0.5;

	//if (triBoxOverlap( &mMidPoint[X], &box_h[X], tri_vertices ))
	if (triBoxOverlap( &mid_point[X], &box_h[X], tri_vertices ))
	{
		if (!mChildren[childID]) mChildren[childID] = new MBOctreeNode( cuboidMin, cuboidMax, mLevel + 1, mMaxLevels );
		mChildren[childID]->AddTriangle( triangleIndex, trianglePoints );
	}
}